/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright
 * 	  notice, this list of conditions and the following disclaimer.
 * 	* Redistributions in binary form must reproduce the above copyright
 *	  notice, this list of conditions and the following disclaimer in the
 * 	  documentation and/or other materials provided with the distribution.
 * 	* Neither the name of the University of California, Berkeley nor the
 * 	  names of its contributors may be used to endorse or promote products
 * 	  derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * @file peripheral.cpp
 * @author haibo
 * @date Sep 1, 2022
 * @brief Example code to control peripheral (jecking, rotary, forklift, roller, etc)
 * Contact: haibo.yang@lgmgim.cn
 */

#include <agv_peripheral_controller/fork/peripheral_fork.h>

using namespace std;
#define BIT(x) (((uint64_t)1) << (x))
#define TestBit(x, i) ((x & BIT(i)) != 0)
#define SET_BIT(x, n) (x | ((uint64_t)(1) << n))
#define CLEAR_BIT(x, n) (x & ~((uint64_t)(1) << n))
#define NEGATION_BIT(x, n) (x ^ ((uint64_t)(1) << n))
namespace agv_peripheral
{

	AgvPeripheral::AgvPeripheral(const std::string &name)
		: name(name), frequency(20.0f), shutdown_req(false),
		  as(nh, "do_peripheral", false)
	// ,dynamic_reconfigure_server(NULL)
	{
		memset(&os, 0, sizeof(os));

		as.registerGoalCallback(boost::bind(&AgvPeripheral::goalCB, this));
		as.registerPreemptCallback(boost::bind(&AgvPeripheral::preemptCB, this));
		as.start();

		load_status = false;
		enable_package_detect = false;
		post_check_enable = false;
		ForkTip_enable = true;
		distance = 0.0;

		read_record_result = false;
		last_set_stop_time = rclcpp::Time::now();

		read_input_data.reset = false;
		read_input_data.emergency = false;
		read_input_data.upLimit = true;
		read_input_data.downLimit = true;
		read_input_data.forkLiftUpLimit = false;

		last_read_input_data.reset = false;
		last_read_input_data.emergency = false;

		zapi_cart_data.current_operation = 0;
		zapi_cart_data.upper_limit = true;
		zapi_cart_data.lower_limit = true;

		yufeng_cart_data.current_operation = 0;
		shielded_cargo_state_detection = false;
	}

	AgvPeripheral::~AgvPeripheral()
	{
		m_quit_mutex.unlock();
		m_thread.join();
		if (peripheral_type == FORKLIFT || YUFENG_CART)
		{
			delete fork_controller;
		}
	}

	void AgvPeripheral::clear_os()
	{
		std::mutex::scoped_lock lock(m_mutex);
		memset(&os, 0, sizeof(os));
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] clear os value");
	}

	void AgvPeripheral::set_cur_op(periph_op_enum op)
	{
		std::mutex::scoped_lock lock(m_mutex);
		if (op)
		{
			operate_time = rclcpp::Time::now();
		}
		os.cur_op = op;
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set op %d", os.cur_op);
	}

	int AgvPeripheral::get_cur_op()
	{
		std::mutex::scoped_lock lock(m_mutex);
		int op = os.cur_op;
		return op;
	}

	void AgvPeripheral::set_next_op(periph_op_enum op)
	{
		std::mutex::scoped_lock lock(m_mutex);
		os.next_op = op;
	}

	int AgvPeripheral::get_next_op()
	{
		std::mutex::scoped_lock lock(m_mutex);
		int op = os.next_op;
		return op;
	}

	void AgvPeripheral::set_cur_state(periph_stata_enum state)
	{
		std::mutex::scoped_lock lock(m_mutex);
		if (os.cur_state != state)
		{
			os.cur_state = state;
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set cur_state %d", os.cur_state);
		}
	}

	int AgvPeripheral::get_cur_state()
	{
		std::mutex::scoped_lock lock(m_mutex);
		int state = os.cur_state;
		return state;
	}

	/** @brief Callback function for the Joint State subscriber */
	void AgvPeripheral::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg)
	{
		joint_state = *msg;
		bool has_fork = (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART);

		// ROS_INFO_STREAM("joint state: " << joint_state);
		if (has_fork)
		{
			if (!find_joint_id)
			{
				bool find_fork = false;
				for (int i = 0; i < (int)(joint_state.name.size()); i++) // find jacking and rotary joint
				{
					if (!find_fork && joint_state.name[i].find(FORKLIFT_JOINT) != string::npos)
					{
						joint_id = i;
						find_fork = true;
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] FORK ID: %d", joint_id);
					}
				}

				if (!find_fork)
					RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Could not find peripheral jonit number");
				else if (find_fork)
					find_joint_id = true;
			}

			if (find_joint_id)
			{
				jointState_update_time = rclcpp::Time::now();
				joint_pos = joint_state.position[joint_id];
				periphMotor[0].pos = joint_pos;
			}
		}
	}

	void AgvPeripheral::check_jointState_out()
	{
		if ((((rclcpp::Time::now() - jointState_update_time).toSec() * 1000) > 1000) && jointState_update_time.toSec()) // 1s
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] check joint state timeout");
			set_cur_state(UNCONTROLLED_STATE);
		}
	}

	void AgvPeripheral::agvModecb(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg)
	{
		uint8_t state = msg->mode;
		input_status.manualState = (state == agv_msgs::msg::AGVMode::MANAUL);
		// input_status.handle_emergency_stop = (state & 1 << 15);
		// input_status.emgButtonState = (state & 1 << 16);
		// input_status.front_and_rear_bumper = (state & 1 << 17);
	}

	void AgvPeripheral::driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &data)
	{
		/*
		driversInfo = *data;
		bool has_jacking = (peripheral_type == JACKING) || (peripheral_type == JACKING_ROTARY);
		bool has_rotary = (peripheral_type == ROTARY) || (peripheral_type == JACKING_ROTARY);

		uint16_t statusWord = 0x0;

		for (int i = 0; i < driversInfo.drivers.size(); i++) {
			statusWord = driversInfo.drivers[i].statusWord;

			if (has_jacking && driversInfo.drivers[i].jointName.find(JACKING_JOINT) != string::npos) {
				motorStatus[0].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[0].homeState = (statusWord & 0x8000);
				motorStatus[0].motorErr = (statusWord & 0x8);
				motorStatus[0].movingState = (statusWord & 0x400);
				motorStatus[0].ready = driversInfo.drivers[i].ready;
				periphMotor[0].enableStatus = motorStatus[0].enableState;
				periphMotor[0].homeFinished = motorStatus[0].homeState;
				periphMotor[0].errStatus = motorStatus[0].motorErr;
				periphMotor[0].ready = motorStatus[0].ready;
				if (statusWord != last_jacking_status_word) {
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking motor statusWord: %x",statusWord);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking statusWord[%x] enable[%d] home[%d]", statusWord, motorStatus[0].enableState, motorStatus[0].homeState);
				}
				last_jacking_status_word = statusWord;
			}

			if (has_rotary && driversInfo.drivers[i].jointName.find(ROTARY_JOINT) != string::npos) {

				motorStatus[0].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[1].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[1].homeState = (statusWord & 0x8000);
				motorStatus[1].motorErr = (statusWord & 0x8);
				motorStatus[1].movingState = (statusWord & 0x400);
				motorStatus[1].ready = driversInfo.drivers[i].ready;
				periphMotor[1].enableStatus = motorStatus[1].enableState;
				periphMotor[1].homeFinished = motorStatus[1].homeState;
				periphMotor[1].errStatus = motorStatus[1].motorErr;
				periphMotor[1].ready = motorStatus[1].ready;
				if (statusWord != last_rotary_status_word) {
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] rotary motor statusWord: %x",statusWord);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] rotary  statusWord[%x] enable[%d] home[%d]", statusWord, motorStatus[1].enableState, motorStatus[1].homeState);
				}
				last_rotary_status_word = statusWord;
			}
		}
		*/
	}

	// 是否时运行时后退
	void AgvPeripheral::ForkTipCheck()
	{
		static bool triggedPre = false;
		bool needCheck = (agv_status.workState == agv_msgs::msg::AgvAutoStatus::IS_NAVIGATE) &&
						 (agv_status.wheelMoveDir == agv_msgs::msg::AgvAutoStatus::BACKWARD_MOVE);
		bool trigged = needCheck &&
					   (read_input_data.leftTipSensor || read_input_data.rightTipSensor 
					|| read_input_data.leftTipTouchSensor || read_input_data.rightTipTouchSensor);
		// ROS_INFO_STREAM("trigged:" << trigged << " needCheck:" << needCheck << " read_input_data.leftTipSensor:" << read_input_data.leftTipSensor <<
		// 				" read_input_data.rightTipSensor:" << read_input_data.rightTipSensor);
		if (ForkTip_enable)
		{
			if (triggedPre != trigged)
			{
				RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] ForkTip Sensor %s while Vehicle move backward !!!", trigged ? "Triged" : "Released");
				triggedPre = trigged;
			}

			// 2023.12.26 modify by haibo
			if (trigged)
			{
				pevent.updateEvent(agv_msgs::msg::AGVEvent::FORK_TIP_EVENT, "ForkTip Sensor Trigged while backward!!!", read_input_data.leftTipSensor, read_input_data.rightTipSensor);
			}
			else if (pevent.checkEvent(agv_msgs::msg::AGVEvent::FORK_TIP_EVENT))
			{
				// if (input_status.reset) // 2024.1.5 not use tip reset
				pevent.removeEvent(agv_msgs::msg::AGVEvent::FORK_TIP_EVENT);
				// else
				// 	pevent.updateEvent(agv_msgs::msg::AGVEvent::FORK_TIP_EVENT, "ForkTip Sensor Trigged while backward!!!");
			}
		}
	}

	void AgvPeripheral::autoStatusCallback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr &msg)
	{
		agv_status = *msg;
	}

	void AgvPeripheral::zapiDiCallback(const std_msgs::msg::UInt16::ConstSharedPtr &msg)
	{
		uint16_t data = msg->data;
		zapi_cart_data.upper_limit = (data >> 11) & 0x1;
		zapi_cart_data.lower_limit = (data >> 12) & 0x1;
	}

	void AgvPeripheral::periphTeleopCtlcb(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg)
	{
		rclcpp::Time now = rclcpp::Time::now();
		uint8_t order = msg->order;
		float freq = msg->freq;
		double speed_limit = 0.0;
		// ROS_INFO_THROTTLE(1, "[AgvPeripheral] teleop called. order[%d] freq[%.4f]  current_state[%d]", order, freq, get_cur_state());

		if ((get_cur_state() != MANUAL_MODE_STATE) || (!freq))
		{
			fork_controller->set_target_velocity(0.0);
			return;
		}

		if (peripheral_type == FORKLIFT)
		{
			if (order == UP)
			{
				if (forklift_manual_active_up == false)
				{
					forklift_manual_start_time_up = now;
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[TELEOPCB] set fork lift up start time: %f", forklift_manual_start_time_up.toSec());
				}

				forklift_manual_active_up = true;
				forklift_timeout_time = 5 * (1 / freq);
				speed_limit = (now - forklift_manual_start_time_up).toSec() * forklift_manual_acc + up_comp;
				manual_target_vel = speed_limit > forklift_manual_vel ? forklift_manual_vel : speed_limit;
				fork_controller->set_target_velocity(manual_target_vel);
				last_forklift_time = now;
			}
			else if (order == DOWN)
			{
				if (forklift_manual_active_down == false)
				{
					forklift_manual_start_time_down = now;
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[TELEOPCB] set fork lift down start time: %f", forklift_manual_start_time_down.toSec());
				}
				forklift_manual_active_down = true;
				forklift_timeout_time = 5 * (1 / freq);
				speed_limit = (now - forklift_manual_start_time_down).toSec() * forklift_manual_acc * -1.0 + down_comp * -1.0;
				manual_target_vel = speed_limit < (forklift_manual_vel * -1.0) ? (forklift_manual_vel * -1.0) : speed_limit;
				fork_controller->set_target_velocity(manual_target_vel);
				last_forklift_time = now;
			}
		}
		else if (peripheral_type == ZAPI_CART)
		{
			agv_msgs::msg::PeriphCustomOperation periph_op;
			if (order == UP && !zapi_cart_data.upper_limit)
			{
				periph_op.operation = 1;
				zapi_cart_data.current_operation = 1;
			}
			else if (order == DOWN && !zapi_cart_data.lower_limit)
			{
				periph_op.operation = 2;
				zapi_cart_data.current_operation = 2;
			}
			else
			{
				periph_op.operation = 0;
				zapi_cart_data.current_operation = 0;
			}

			ROS_INFO_THROTTLE(1.0, "[TELEOPCB] order: %d  upper_limit: %d  lower_limit: %d", order, zapi_cart_data.upper_limit, zapi_cart_data.lower_limit);
			custom_operation_pub.publish(periph_op);
		}
		else if (peripheral_type == YUFENG_CART)
		{
			if (order == UP && !read_input_data.upLimit)
			{
				yufeng_cart_data.current_operation = 1;
				fork_controller->set_target_velocity(100.0);
			}
			else if (order == DOWN && !read_input_data.downLimit)
			{
				yufeng_cart_data.current_operation = 2;
				fork_controller->set_target_velocity(-100.0);
			}
			else
			{
				yufeng_cart_data.current_operation = 0;
				fork_controller->set_target_velocity(0.0);
			}

			ROS_INFO_THROTTLE(1.0, "[TELEOPCB] order: %d  upper_limit: %d  lower_limit: %d", order, read_input_data.upLimit, read_input_data.downLimit);
		}
	}

	void AgvPeripheral::readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg)
	{
		std::vector<agv_msgs::msg::PairTypeInt> input_msg = msg->array;

		for (const auto &data : input_msg)
		{
			if (data.key.compare("resetButton") == 0)
			{
				read_input_data.reset = (data.value == 1);
			}

			if (data.key.compare(0, 15, "emergencyButton") == 0)
			{
				read_input_data.emergency = (data.value == 1);
			}

			if (data.key.compare("leftLoadInplace") == 0)
			{
				read_input_data.leftLoadInplace = (data.value == 1);
			}

			if (data.key.compare("rightLoadInplace") == 0)
			{
				read_input_data.rightLoadInplace = (data.value == 1);
			}

			if (data.key.compare("loadStatus") == 0)
			{
				read_input_data.loadStatus = (data.value == 1);
			}

			if (data.key.compare("leftForkTipSensor") == 0)
			{
				read_input_data.leftTipSensor = (data.value == 1);
			}

			if (data.key.compare("rightForkTipSensor") == 0)
			{
				read_input_data.rightTipSensor = (data.value == 1);
			}
			if (data.key.compare("leftForkTipTouchSensor") == 0)
			{
				read_input_data.leftTipTouchSensor = (data.value == 1);
			}

			if (data.key.compare("rightForkTipTouchSensor") == 0)
			{
				read_input_data.rightTipTouchSensor = (data.value == 1);
			}
			if (data.key.compare("upLimit") == 0)
			{
				read_input_data.upLimit = (data.value == 1);
			}

			if (data.key.compare("downLimit") == 0)
			{
				read_input_data.downLimit = (data.value == 1);
			}

			if (data.key.compare("forkLiftUpLimit") == 0)
			{
				read_input_data.forkLiftUpLimit = (data.value == 1);
				if (read_input_data.forkLiftUpLimit)
				{
					ROS_INFO_STREAM("fork lift reach up limit");
				}
			}
		}

		load_status = read_input_data.loadStatus || (read_input_data.leftLoadInplace & read_input_data.rightLoadInplace);
	}

	/*
	void AgvPeripheral::readInputCallback(const agv_msgs::msg::ReadInPuts::ConstSharedPtr &msg)
	{
		if (peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
		{
			uint64_t input = msg->ret_bits;
			// 2024.01.19 load status check modify by haibo
			bool leftloadinplace = ((input >> 33) & 0x1 == 0x1);
			bool rightloadinplace = ((input >> 34) & 0x1 == 0x1);
			bool loadsingle = ((input >> 35) & 0x1 == 0x1);
			loadStatus = loadsingle || (leftloadinplace && rightloadinplace);
			leftTipSensor = ((input >> 31) & 0x1 == 0x1);
			rightTipSensor = ((input >> 32) & 0x1 == 0x1);

			input_status.reset = (input & 0x02) != 0;
			// fixbug:两路急停信号等价-以下变量
			input_status.handle_emergency_stop = TestBit(input, 2); //(input & 1 << 2);
			input_status.emgButtonState = TestBit(input, 12);		//(input & 1 << 12);

			read_input_data.reset = input_status.reset;
			read_input_data.emergency_stop_1 = input_status.handle_emergency_stop;
			read_input_data.emergency_stop_2 = input_status.emgButtonState;
			ROS_INFO_STREAM_THROTTLE(0.5, "rets=" << (int)msg->ret_bits << ",input=" << (int)input
												  << ",handle_emergency_stop=" << input_status.handle_emergency_stop << "emgButtonState=" << input_status.emgButtonState
												  << ",emergency_stop_1=" << read_input_data.emergency_stop_1 << ",emergency_stop_2=" << read_input_data.emergency_stop_2
												  << ",input_status.reset=" << input_status.reset << ",reset=" << read_input_data.reset

			);
			// input_status.front_and_rear_bumper = (input & 1 << 17);
		}
	}
	*/

	void AgvPeripheral::odomCallback(const agv_msgs::msg::OdometryLite::ConstSharedPtr &msg)
	{
		odom_data = *msg;
		last_odom_time = rclcpp::Time::now();
	}

	void AgvPeripheral::poseCallback(const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr &msg)
	{
		current_pose = *msg;
	}

	void AgvPeripheral::set_stop_bits()
	{
		if (peripheral_type == FORKLIFT)
		{
			int fork_state = fork_controller->get_state();
			if (fork_state == ForkController::TYPE::TASK)
			{
				setStop.stop = 1;
				if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.02)
				{
					set_stop_pub.publish(setStop);
					last_set_stop_time = rclcpp::Time::now();
				}
			}
			else
			{
				setStop.stop = 0;
				if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.1)
				{
					set_stop_pub.publish(setStop);
					last_set_stop_time = rclcpp::Time::now();
				}
			}
		}
		else if (peripheral_type == ZAPI_CART)
		{
			if (!zapi_cart_data.lower_limit && !zapi_cart_data.upper_limit)
			{
				setStop.stop = 1;
				if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.02)
				{
					set_stop_pub.publish(setStop);
					last_set_stop_time = rclcpp::Time::now();
				}
			}
			else
			{
				setStop.stop = 0;
				if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.1)
				{
					set_stop_pub.publish(setStop);
					last_set_stop_time = rclcpp::Time::now();
				}
			}
		}
		else if (peripheral_type == YUFENG_CART)
		{
			if (!read_input_data.upLimit && !read_input_data.downLimit)
			{
				setStop.stop = 1;
				if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.02)
				{
					set_stop_pub.publish(setStop);
					last_set_stop_time = rclcpp::Time::now();
				}
			}
			else
			{
				setStop.stop = 0;
				if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.1)
				{
					set_stop_pub.publish(setStop);
					last_set_stop_time = rclcpp::Time::now();
				}
			}
		}
	}

	void AgvPeripheral::drop_post_check(bool enable)
	{
		static bool last_enable_state = false;
		static double cpose[3] = {0.0f};

		if (enable)
		{
			rclcpp::Time timestamp = rclcpp::Time::now();
			// check odom topic
			if ((timestamp - last_odom_time).toSec() > 0.1f)
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "odom timeout. now: %.4f  odom: %.4f", timestamp.toSec(), last_odom_time.toSec());
				pevent.updateEvent(agv_msgs::msg::AGVEvent::TOPIC_TIME_OUT, "Odometry data timeout !!!");
				return;
			}
			else if (pevent.checkEvent(agv_msgs::msg::AGVEvent::TOPIC_TIME_OUT))
			{
				pevent.removeEvent(agv_msgs::msg::AGVEvent::TOPIC_TIME_OUT);
			}

			if (last_enable_state != enable)
			{
				if (enable)
				{ // get current pose
					cpose[0] = odom_data.pose.position.x;
					cpose[1] = odom_data.pose.position.y;
				}

				last_enable_state = enable;
			}

			distance = std::sqrt(pow(odom_data.pose.position.x - cpose[0], 2) + pow(odom_data.pose.position.y - cpose[1], 2));
			ROS_INFO_THROTTLE(0.5, "[AgvPeripheral] Distance after DROP: %.4f", distance);

			if (distance > drop_detect_distance)
			{
				{
					std::mutex::scoped_lock lock(m_mutex1);
					post_check_enable = false;
					distance = 0.0;
					shielded_cargo_state_detection = false;
					last_enable_state = false;
				}

				// 2023.12.26 modify by haibo
				if (load_status)
				{
					RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] detect package after drop !!!");
					pevent.updateEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR, "Detect Package After Drop !!!");
				}
				// Screened cargo detection
			}
			else if (get_cur_state() == IDLE_STATE)
			{
				shielded_cargo_state_detection = true;
			}

			last_odom_time = odom_data.header.stamp;
		}
		else if (pevent.checkEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR))
		{
			if (read_input_data.reset) // manual reset
				pevent.removeEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR);
			else
				pevent.updateEvent(agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR, "Detect Package After Drop !!!");
		}
		else
		{
			last_enable_state = false;
			shielded_cargo_state_detection = false;
		}
	}

	bool AgvPeripheral::init()
	{
		int type;
		nh.param("/agv_peripheral_controller/PeripheralType", type, 4);
		peripheral_type = (Type)type;

		if (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "FORKLIFT/YUFENG_CART");
			periphMotor.resize(1);
			periphMotor[0].motor_name = FORKLIFT_JOINT;
			nh.param<float>("/agv_peripheral_controller/ForkLiftMaxPos", max_pos, 3.00);
			nh.param<float>("/agv_peripheral_controller/ForkLiftMinPos", min_pos, 0.00);
			nh.param("/agv_peripheral_controller/ForkLiftManualAcc", forklift_manual_acc, 50.0);
			nh.param("/agv_peripheral_controller/ForkLiftManualVel", forklift_manual_vel, 80.0);
			nh.param("/agv_peripheral_controller/ForkLiftPackDetect", enable_package_detect, false);
			nh.param("/agv_peripheral_controller/ForkLiftDist", drop_detect_distance, 1.2);
			nh.param("/agv_peripheral_controller/ForkTip_enable", ForkTip_enable, true);
			nh.param<int>("/agv_peripheral_controller/ForkLiftUPComp", up_comp, 30);
			nh.param<int>("/agv_peripheral_controller/ForkLiftDOWNComp", down_comp, 30);

			fork_controller = new ForkController();
		}
		else if (peripheral_type == ZAPI_CART)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "ZAPI_CART");
			periphMotor.resize(1);
			periphMotor[0].motor_name = ZAPI_JOINT;
			custom_operation_pub = nh.advertise<agv_msgs::msg::PeriphCustomOperation>("/zapi_periph_operation", 10);
			zapi_di_sub = nh.subscribe("/zapi_digital_input", 10, &AgvPeripheral::zapiDiCallback, this);
			nh.param("/agv_peripheral_controller/ForkLiftPackDetect", enable_package_detect, false);
			nh.param("/agv_peripheral_controller/ForkLiftDist", drop_detect_distance, 1.2);
			nh.param("/agv_peripheral_controller/ForkTip_enable", ForkTip_enable, true);
		}

		// 2023.12.26 add by haibo
		pevent.init(nh);

		// subscribe joint state
		js_sub = nh.subscribe("/base/joint_states", 1, &AgvPeripheral::jointStateCallback, this);
		// peripheralGetcontrollers = nh.serviceClient<controller_manager_msgs::srv::ListControllers>("/base/controller_manager/list_controllers");
		periph_status_pub = nh.advertise<agv_msgs::msg::PeriphStatus>("/periphStatus", 1);
		periph_teleop_sub = nh.subscribe("/base/periphTeleopInfo", 1, &AgvPeripheral::periphTeleopCtlcb, this);
		agvmode_sub = nh.subscribe("agvmode", 1, &AgvPeripheral::agvModecb, this);
		PeripheralHaltCtl = nh.serviceClient<std_srvs::srv::Trigger>("/base/driver/halt");
		PeripheralRecoverCtl = nh.serviceClient<std_srvs::srv::Trigger>("/base/driver/recover");
		driver_info_sub = nh.subscribe("/base/driversInfo", 1, &AgvPeripheral::driversInfoCallback, this);
		set_stop_pub = nh.advertise<agv_msgs::msg::SetStopBits>("/set_stop_bits", 10);
		auto_status_sub = nh.subscribe("/auto_run_status", 10, &AgvPeripheral::autoStatusCallback, this);
		// readinput_sub = nh.subscribe("/readInput", 10, &AgvPeripheral::readInputCallback, this);
		readinput_sub = nh.subscribe("/readInputNew", 10, &AgvPeripheral::readInputCallback, this);

		odom_sub = nh.subscribe("/odom", 10, &AgvPeripheral::odomCallback, this);
		pose_sub = nh.subscribe("/pose", 1, &AgvPeripheral::poseCallback, this);

		// pos_record_file = ros::package::getPath(PACKAGE_NAME) + FILE_NAME;
		pos_record_file = std::string(getenv("HOME")) + FILE_NAME;
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "pos record file path is: %s", pos_record_file.c_str());

		read_record_result = periphRecordPosRead();
		m_quit_mutex.lock();
		m_thread = std::thread(thread_fun, this);
		return true;
	}

	bool AgvPeripheral::periphReadyCheck()
	{
		bool periphReady = false;
		if (peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
		{
			periphReady = true;
		}

		return periphReady;
	}

	bool AgvPeripheral::periphMotorEnableCheck()
	{
		bool periphEnabled = false;
		if (peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
		{
			periphEnabled = true;
		}

		return periphEnabled;
	}

	bool AgvPeripheral::periphMotorErrCheck()
	{
		bool periphinErr = false;
		if (peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
		{
			periphinErr = false;
		}

		return periphinErr;
	}

	bool AgvPeripheral::periphHomingCheck()
	{
		bool periphHomed = false;
		if (peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
		{
			periphHomed = true;
		}

		return periphHomed;
	}

	void AgvPeripheral::periphStatusGather()
	{
		int state = get_cur_state();
		if (peripheral_type == FORKLIFT)
			ROS_INFO_THROTTLE(0.5, "state: %d  joint_pos: %.4f  load_status: %d  post_check_enable: %d", state, joint_pos, load_status, post_check_enable);

		rclcpp::Time time_now = rclcpp::Time::now();
		periphStatus.header.stamp = time_now;

		if (peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
		{
			periphStatus.loaded = shielded_cargo_state_detection ? 0 : load_status;
		}

		// 如果是地牛,则设置loadtype数据，因为地牛只有最低 最高两个高度
		std::string loadType = "0";
		if (peripheral_type == ZAPI_CART) {
			loadType = zapi_cart_data.lower_limit ? "0" : "1";
		}
		else if (peripheral_type == YUFENG_CART) {
			loadType = read_input_data.downLimit ? "0" : "1";
		}
		else if (peripheral_type == FORKLIFT) {
			loadType = periphStatus.loaded ? "1" : "0";
		}

		// else if (peripheral_type == ZAPI_CART)
		// {
		// 	if (zapi_cart_data.lower_limit)
		// 		periphStatus.loaded = false;
		// 	else
		// 		periphStatus.loaded = true;
		// }

		loads.clear();
		periphStatus.loads.clear();
		loadInfo.loadId = "";
		loadInfo.loadType = loadType;
		loadInfo.loadPosition = periphStatus.loaded ? "1" : "0";

		loads.push_back(loadInfo);
		periphStatus.loads = loads;

		periphStatus.periphStateInfo = state;
		periphMotor[0].enableStatus = true;
		periphMotor[0].homeFinished = true;
		periphMotor[0].errStatus = false;
		periphMotor[0].ready = true;
		periphStatus.periphMotor = periphMotor;
		periph_status_pub.publish(periphStatus);
	}

	bool AgvPeripheral::periphGetControllerRunningStatus()
	{
		return true;
		// bool controllerIsRunning = false;
		// controller_manager_msgs::srv::ListControllers srv;
		// if (peripheralGetcontrollers.call(srv))
		// {
		// 	for (int i = 0; i < (int)(srv.response.controller.size()); i++)
		// 	{
		// 		controller_manager_msgs::msg::ControllerState controller = srv.response.controller.at(i);

		// 		if (peripheral_type == FORKLIFT) {
		// 			if (controller.name == "fork_controller") {
		// 				if (controller.state == "running") {
		// 					controllerIsRunning = true;
		// 				} else {
		// 					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "fork_controller is %s", controller.state.c_str());
		// 				}
		// 			}
		// 		}
		// 	}
		// }

		// if (peripheral_type == FORKLIFT)
		// 	return controllerIsRunning;
		// else if (peripheral_type == ZAPI_CART)
		// 	return true;

		// return false;
	}

	bool AgvPeripheral::periphMoveToRecordPos()
	{

		if (!read_record_result)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "cannot read record. move to record cancled");
			return true;
		}

		return true;
		// ROS_INFO_THROTTLE(0.2, "read file jacking position: %f  current jacking position: %f", file_record_pos[0], joint_pos[0]);
		// RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "read file jacking position: %f  current jacking position: %f", file_record_pos[0], joint_pos[0]);

		/*
		if (peripheral_type == JACKING || peripheral_type == JACKING_ROTARY) {
			if (file_record_pos[0] > max_pos[0] || file_record_pos[0] < min_pos[0]) {
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] bad record, quit");
				return true;
			}

			if ((abs(file_record_pos[0]) <= 0.0001) && (abs(file_record_pos[1]) <= 0.0001))
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] record home. finished");
				return true;
			}

			if(abs(joint_pos[0] - file_record_pos[0]) <= 0.0001)
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] reach record pos. finished");
				jacking_arrive_recordPos = true;
				jacking_move_state = false;
			}


			if ((!jacking_move_state) && (abs(joint_pos[0] - file_record_pos[0]) > 0.0001))
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking move to record pos, publish: %f current_pos: %f", file_record_pos[0], joint_pos[0]);
				pallet_controller->set_jacking_target_position(file_record_pos[0]);
				jacking_move_state = true;
				jacking_move_time = rclcpp::Time::now();
			}


			if (jacking_move_state && ((rclcpp::Time::now() - jacking_move_time).toSec() > OPERATE_TIMEOUT_S))
			{
				if (motorStatus[0].movingState && abs(joint_pos[0] - file_record_pos[0]) > 0.0001)
				{
					pallet_controller->set_jacking_target_position(joint_pos[0]);
					RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "jacking move to record pos timeout");
					jacking_move_state = false;
				}
			}
		}

		return jacking_arrive_recordPos;//  & rotary_arrive_recordPos;
		*/
	}

	void AgvPeripheral::periphStateMachineUpdate()
	{
		if (!periphInitialized)
		{
			ROS_INFO_ONCE("PeripheralType: %d", peripheral_type);
			if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck())
			{
				// fork does not need to move to record pos.
				// if(periphMoveToRecordPos())
				// {
				periphInitialized = true;
				set_cur_state(IDLE_STATE);
				// }
			}
			else
			{
				ROS_INFO_THROTTLE(0.5, "[AgvPeripheral] controller status check fail");
				set_cur_state(UNCONTROLLED_STATE);
			}
		}
		if (periphInitialized)
		{
			switch (get_cur_state())
			{
			case IDLE_STATE:
				if (input_status.manualState)
				{
					if (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART)
					{
						fork_controller->stop();
					}
					else if (peripheral_type == ZAPI_CART)
					{
						agv_msgs::msg::PeriphCustomOperation periph_op;
						periph_op.operation = 0;
						custom_operation_pub.publish(periph_op);
					}

					set_cur_state(MANUAL_MODE_STATE);
				}
				break;
			case MANUAL_MODE_STATE:

				if (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART)
				{
					if (read_input_data.emergency == 1 && last_read_input_data.emergency == 0)
					{
						if (PeripheralHaltCtl.call(trig_srv))
						{
							set_cur_state(HALT_HANDLE_STATE);
						}
						break;
					}
				}

				if (!input_status.manualState)
				{
					fork_controller->stop();
					set_cur_state(IDLE_STATE);
				}
				else
				{
					rclcpp::Time now = rclcpp::Time::now();

					if (peripheral_type == FORKLIFT)
					{
						if ((now - last_forklift_time).toSec() > forklift_timeout_time)
						{
							if (forklift_manual_active_up)
							{
								forklift_manual_stop_time_up = now;
								forklift_manual_stop_flag_up = true;
								forklift_manual_active_up = false;
								RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] teleop forklift up timeout. now[%.4f] last[%.4f]", now.toSec(), last_forklift_time.toSec());
							}

							if (forklift_manual_active_down)
							{
								forklift_manual_stop_time_down = now;
								forklift_manual_stop_flag_down = true;
								forklift_manual_active_down = false;
								RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] teleop forklift down timeout. now[%.4f] last[%.4f]", now.toSec(), last_forklift_time.toSec());
							}

							if (forklift_manual_stop_flag_up)
							{
								double speed = manual_target_vel - (now - forklift_manual_stop_time_up).toSec() * forklift_manual_acc;
								ROS_INFO_THROTTLE(0.1, "[FORKLIFT] manual_target: %.4f  dt: %f  acc: %.4f  speed: %.4f",
												  manual_target_vel, (now - forklift_manual_stop_time_up).toSec(), forklift_manual_acc, speed);

								if (speed < 0.0) {
									forklift_manual_stop_flag_up = false;
									fork_controller->set_target_velocity(0.0);
								}
								else {
									ROS_INFO_THROTTLE(0.1, "[FORKLIFT] last_target: %.4f  set speed %.4f", manual_target_vel, speed);
									fork_controller->set_target_velocity(speed);
								}
							}

							if (forklift_manual_stop_flag_down)
							{
								double speed = manual_target_vel + (now - forklift_manual_stop_time_down).toSec() * forklift_manual_acc;
								ROS_INFO_THROTTLE(0.1, "[FORKLIFT] manual_target: %.4f  dt: %f  acc: %.4f  speed: %.4f",
												  manual_target_vel, (now - forklift_manual_stop_time_down).toSec(), forklift_manual_acc, speed);
								if (speed > 0.0) {
									forklift_manual_stop_flag_down = false;
									fork_controller->set_target_velocity(0.0);
								}
								else {
									ROS_INFO_THROTTLE(0.1, "[FORKLIFT] last_target: %.4f  set speed %.4f", manual_target_vel, speed);
									fork_controller->set_target_velocity(speed);
								}
							}
						}
						else
						{
							forklift_manual_stop_flag_up = false;
							forklift_manual_stop_flag_down = false;
						}
					}
					else if (peripheral_type == ZAPI_CART)
					{
						agv_msgs::msg::PeriphCustomOperation periph_op;
						if ((zapi_cart_data.current_operation == 1 && zapi_cart_data.upper_limit) ||
							(zapi_cart_data.current_operation == 2 && zapi_cart_data.lower_limit))
						{
							if (zapi_cart_data.upper_limit)
								RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] ZAPI_CART  reach upper limit");
							if (zapi_cart_data.lower_limit)
								RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] ZAPI_CART  reach lower limit");

							zapi_cart_data.current_operation = 0;
							periph_op.operation = 0;
							custom_operation_pub.publish(periph_op);
						}
					}
					else if (peripheral_type == YUFENG_CART)
					{
						if ((yufeng_cart_data.current_operation == 1 && read_input_data.upLimit) ||
							(yufeng_cart_data.current_operation == 2 && read_input_data.downLimit))
						{
							if (read_input_data.upLimit)
								RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] YUFENG_CART  reach up limit");
							if (read_input_data.upLimit)
								RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] YUFENG_CART  reach down limit");

							yufeng_cart_data.current_operation = 0;
							fork_controller->set_target_velocity(0.0);
						}
					}
				}
				break;
			case HALT_HANDLE_STATE:
			{
				// 挂起模式停止运行
				if (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART)
				{
					fork_controller->stop();
				}
				else if (peripheral_type == ZAPI_CART)
				{
					agv_msgs::msg::PeriphCustomOperation periph_op;
					periph_op.operation = 0;
					custom_operation_pub.publish(periph_op);
				}
				// 挂起模式停止运行

				// if (!input_status.emgButtonState && !input_status.handle_emergency_stop && !input_status.front_and_rear_bumper)
				if (read_input_data.reset == 1 && last_read_input_data.reset == 0 &&
					read_input_data.emergency == 0)
				{
					if (PeripheralRecoverCtl.call(trig_srv))
					{
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover success");
						set_cur_state(IDLE_STATE);
					}
					else
					{
						RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover fail");
					}
				}
			}
			break;
			case FORKLIFT_OP_STATE:
			case FORKLIFT_SUCCESS_STATE:
				
				if (read_input_data.forkLiftUpLimit)
				{
					fork_controller->stop();
					set_cur_state(IDLE_STATE);
					result.success = FAILED;
					result.message = "Operate halt";
					as.setSucceeded(result);
					set_cur_op(NO_OP);
					set_next_op(NO_OP);
				}
				
				if (read_input_data.emergency)
				{
					set_cur_state(HALT_HANDLE_STATE);
					if (PeripheralHaltCtl.call(trig_srv))
					{
						result.success = FAILED;
						result.message = "Operate halt";
						as.setSucceeded(result);
						set_cur_op(NO_OP);
						set_next_op(NO_OP);
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call halt ser success,operation is Completed");
					}
					else
					{
						RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover ser fail");
					}
				}
				break;
			case UNCONTROLLED_STATE:
				if (get_cur_op())
				{
					result.success = FAILED;
					result.message = "Operate fail";
					as.setSucceeded(result);
					set_cur_op(NO_OP);
					set_next_op(NO_OP);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] operation is uncompleted, motor is uncontrollable");
				}

				if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck() && !periphMotorErrCheck())
				{
					set_cur_state(IDLE_STATE);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] uncontrolled state recovered, set idle state");
				}

				break;
			default:
				break;
			}

			if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck() && !periphMotorErrCheck())
			{
			}
			else
			{

				if (get_cur_state() != HALT_HANDLE_STATE)
				{
					set_cur_state(UNCONTROLLED_STATE);
					ROS_INFO_THROTTLE(1.0, "[AgvPeripheral] motor check error, set uncontrolled state");
				}
			}

			last_read_input_data.reset = read_input_data.reset;
			last_read_input_data.emergency = read_input_data.emergency;
		}
	}

	void AgvPeripheral::spin()
	{
		// double period = 1.0 / frequency;
		rclcpp::Rate spin_rate(frequency);

		while (!shutdown_req && rclcpp::ok())
		{
			if (get_cur_op())
			{
				switch (get_cur_op())
				{
				case LIFT:

					bool finished = false;
					if (peripheral_type == FORKLIFT)
					{
						finished = abs(joint_pos - target_pos) < 0.005;
					}
					else if (peripheral_type == ZAPI_CART)
					{
						if (target_pos > 0.0)
						{
							finished = zapi_cart_data.upper_limit;
						}
						else if (target_pos < 0.0)
						{
							finished = zapi_cart_data.lower_limit;
						}
					}
					else if (peripheral_type == YUFENG_CART)
					{
						if (target_pos > 0.0)
						{
							finished = read_input_data.upLimit;
						}
						else if (target_pos < 0.0)
						{
							finished = read_input_data.downLimit;
						}
					}

					if (finished)
					{
						if (peripheral_type == ZAPI_CART)
						{
							if ((zapi_cart_data.current_operation == 1 && zapi_cart_data.upper_limit) ||
								(zapi_cart_data.current_operation == 2 && zapi_cart_data.lower_limit))
							{
								if (zapi_cart_data.upper_limit)
									RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] ZAPI_CART  reach upper limit");
								if (zapi_cart_data.lower_limit)
									RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] ZAPI_CART  reach lower limit");

								agv_msgs::msg::PeriphCustomOperation periph_op;
								zapi_cart_data.current_operation = 0;
								periph_op.operation = 0;
								custom_operation_pub.publish(periph_op);
							}
						}
						else if (peripheral_type == YUFENG_CART)
						{
							if ((yufeng_cart_data.current_operation == 1 && read_input_data.upLimit) ||
								(yufeng_cart_data.current_operation == 2 && read_input_data.downLimit))
							{
								if (read_input_data.upLimit)
									RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] YUFENG_CART  reach up limit");
								if (read_input_data.downLimit)
									RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] YUFENG_CART  reach down limit");

								yufeng_cart_data.current_operation = 0;
								fork_controller->set_target_velocity(0.0);
							}
						}

						set_cur_state(FORKLIFT_SUCCESS_STATE);
						if (get_next_op())
						{
							feedback.state = get_cur_state();
							feedback.percent_complete = 1.0f;
							feedback.feedback_action = current_action;
							as.publishFeedback(feedback);
							set_cur_op((periph_op_enum)get_next_op());
							set_next_op(NO_OP);
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] FORKLIFT Complete and Next Operate:%d", os.next_op);
							executeOP(get_cur_op());
						}
						else
						{
							result.success = SUCCESS;
							result.message = "Operate successed";
							result.result_action = current_action;
							as.setSucceeded(result);
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] FORKLIFT Complete");
							clear_os();
						}
					}
					else
					{
						feedback.state = get_cur_state();
						feedback.percent_complete = abs(joint_pos - previous_pos) * 1.0f / abs(max_pos - previous_pos);
						feedback.feedback_action = current_action;
						if (peripheral_type == FORKLIFT)
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] FORKLIFT Up Complete:%.2f,curent pos is %.4f", feedback.percent_complete, joint_pos);
						as.publishFeedback(feedback);
					}
					break;
				}
			}
			if (get_cur_op() == LIFT)
			{
				if ((rclcpp::Time::now() - operate_time).toSec() > OPERATE_TIMEOUT_S)
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] now time is %f,operate_time is %f", rclcpp::Time::now().toSec(), operate_time.toSec());

					if ((rclcpp::Time::now() - operate_time).toSec() > MOVE_TIMEOUT_S)
					{
						clear_os();
						result.success = FAILED;
						result.message = "move timeout";
						result.result_action = current_action;
						as.setSucceeded(result);
						RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] FORKLIFT move timeout");
					}
				}
			}

			periphStateMachineUpdate();

			periphStatusGather();

			if (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART)
			{
				fork_controller->update();

				drop_post_check(post_check_enable);

				if (ForkTip_enable)
				{
					ForkTipCheck();
				}
			}
			else if (peripheral_type == ZAPI_CART)
			{
				drop_post_check(post_check_enable);

				if (ForkTip_enable)
				{
					ForkTipCheck();
				}
			}

			rclcpp::spin_some(node);
			spin_rate.sleep();
		}
	}

	bool AgvPeripheral::executeOP(int order)
	{
		bool success = true;
		switch (order)
		{
		case LIFT:
			if (peripheral_type == FORKLIFT)
			{
				previous_pos = joint_pos;
				fork_controller->set_target_position(target_pos);
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set order up, target pos is %.4f", max_pos);
				set_cur_state(FORKLIFT_OP_STATE);
			}
			else if (peripheral_type == ZAPI_CART)
			{
				agv_msgs::msg::PeriphCustomOperation periph_op;
				if (target_pos > 0 && !zapi_cart_data.upper_limit)
				{
					periph_op.operation = 1;
					zapi_cart_data.current_operation = 1;
				}
				else if (target_pos < 0 && !zapi_cart_data.lower_limit)
				{
					periph_op.operation = 2;
					zapi_cart_data.current_operation = 2;
				}
				else
				{
					periph_op.operation = 0;
					zapi_cart_data.current_operation = 0;
				}

				custom_operation_pub.publish(periph_op);
			}
			else if (peripheral_type == YUFENG_CART)
			{
				if (target_pos > 0 && !read_input_data.upLimit)
				{
					yufeng_cart_data.current_operation = 1;
					fork_controller->set_target_velocity(100.0);
				}
				else if (target_pos < 0 && !read_input_data.downLimit)
				{
					yufeng_cart_data.current_operation = 2;
					fork_controller->set_target_velocity(-100.0);
				}
				else
				{
					yufeng_cart_data.current_operation = 0;
					fork_controller->set_target_velocity(0.0);
				}
			}
			break;

		default:
			success = false;
			break;
		}

		if (!success)
		{
			result.success = FAILED;
			result.message = "Operation not defined";
			result.result_action = current_action;
			as.setSucceeded(result);
			set_cur_state(IDLE_STATE);
			RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Operate failed");
			return false;
		}
		return true;
	}

	void AgvPeripheral::goalCB()
	{

		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB received");
		if (as.isNewGoalAvailable())
		{
			auto goal_ptr = as.acceptNewGoal();
			if (goal_ptr.get())
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB get pointer");
				current_action = goal_ptr->goal_action;
			}
			else
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB get null pointer");

				result.success = FAILED;
				result.message = "Operate fail: bad pointer";
				as.setSucceeded(result);
				return;
			}
		}
		else
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB  no goal available!");
			result.success = FAILED;
			result.message = "Operate fail: bad pointer";
			as.setSucceeded(result);
			return;
		}

		ROS_INFO_STREAM("goal action: " << current_action);
		// int action_id = atoi(current_action.actionId.c_str());
		bool use_forklift = (current_action.actionType.compare(action_pick) == 0) || (current_action.actionType.compare(action_drop) == 0) || (current_action.actionType.compare(action_adjust) == 0);

		if ((peripheral_type == FORKLIFT || peripheral_type == ZAPI_CART) && enable_package_detect)
		{
			if (current_action.actionType.compare(action_pick) == 0)
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] PICK pre check, current_pose: x[%.6f] y[%.6f]", current_pose.pose.pose.position.x, current_pose.pose.pose.position.y);
				if (!load_status)
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] detect no package before pick");
					result.success = FAILED;
					result.message = "FAILED detect no package before pick";
					as.setSucceeded(result);
					return;
				}
			}
			else if (current_action.actionType.compare(action_drop) == 0)
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] DROP pre check, current_pose: x[%.6f] y[%.6f]", current_pose.pose.pose.position.x, current_pose.pose.pose.position.y);
				if (!load_status)
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] detect no package before drop");
					result.success = FAILED;
					result.message = "FAILED detect no package before drop";
					as.setSucceeded(result);
					return;
				}

				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] start distance detect");
				std::mutex::scoped_lock lock(m_mutex1);
				post_check_enable = true;
				distance = 0.0;
			}
		}

		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] action received. action type: %s  use_forklift: %d", current_action.actionType.c_str(), use_forklift);

		if (get_cur_state() != IDLE_STATE)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB device busy");
			result.success = FAILED;
			result.message = "ERROR Device is busy";
			result.result_action = current_action;
			as.setSucceeded(result);
			RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] last task is not completed");
			return;
		}

		int action_id = 0;
		if (use_forklift)
		{
			action_id = LIFT;
			for (int i = 0; i < (int)(current_action.actionParameters.size()); i++)
			{
				if (peripheral_type == FORKLIFT)
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB:  search height");
					if (current_action.actionParameters[i].key.compare(string("height")) == 0)
					{
						bool fail = false;
						if (current_action.actionParameters[i].value.length() == 0)
						{
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB:  bad value length");
							fail = true;
						}

						try
						{
							target_pos = std::stof(current_action.actionParameters[i].value);
						}
						catch (...)
						{
							fail = true;
							RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "error: string2float failed");
						}

						if (target_pos > max_pos || target_pos < min_pos)
						{
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB:  bad value");
							fail = true;
						}

						if (fail)
						{
							result.success = FAILED;
							result.message = "Operate fail: bad target";
							as.setSucceeded(result);
							return;
						}
					}
				}
				else if (peripheral_type == ZAPI_CART || peripheral_type == YUFENG_CART)
				{
					if (current_action.actionType.compare(action_pick) == 0)
					{
						target_pos = 1.0;
					}
					else if (current_action.actionType.compare(action_drop) == 0)
					{
						target_pos = -1.0;
					}
				}
			}
		}

		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB: action_id[%d]", action_id);
		switch (action_id)
		{
		case NO_OP:
		case LIFT:
			set_cur_op((periph_op_enum)action_id);
			set_next_op(NO_OP);
			break;

		default:
			break;
		}

		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB: execute action_id[%d]", action_id);
		executeOP(action_id);
	}

	void AgvPeripheral::preemptCB()
	{
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] %s: Preempted", name.c_str());
		result.result_action = current_action;

		if (as.isActive())
		{
			if (get_cur_op() == LIFT)
			{
				if (peripheral_type == FORKLIFT || peripheral_type == YUFENG_CART)
				{
					fork_controller->stop();
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] cancel the forklift joint goal");
				}
				else if (peripheral_type == ZAPI_CART)
				{
					agv_msgs::msg::PeriphCustomOperation periph_op;
					periph_op.operation = 0;
					custom_operation_pub.publish(periph_op);
				}
			}

			clear_os();
		}

		return;
		// set the action state to preempted
		result.success = CANCELED;
		result.message = "Action has been preempted";
		result.result_action = current_action;
		as.setPreempted(result);
	}

	bool AgvPeripheral::periphRecordPosRead()
	{
		std::fstream fin(pos_record_file.c_str(), std::ios::in);
		if (fin.fail())
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "could not open %s.", pos_record_file.c_str());
			return false;
		}

		try
		{
			YAML::Node yamlConfig = YAML::Load(fin);
			fin.close();
			if (!yamlConfig)
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "read %s faild. data is not yaml format", pos_record_file.c_str());
				return false;
			}
			if (yamlConfig["forklift_joint_pos"])
			{
				file_record_pos = yamlConfig["forklift_joint_pos"].as<float>();
				if (file_record_pos > max_pos)
					file_record_pos = max_pos;
				if (file_record_pos < min_pos)
					file_record_pos = min_pos;
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "read pos success, forklift joint pos is %.4f", file_record_pos);
			}
			else
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "read %s faild. data is not exist", pos_record_file.c_str());
				return false;
			}
		}
		catch (const YAML::Exception &e)
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "file format error %s,%s.", pos_record_file.c_str(), e.what());
			return false;
		}
		return true;
	}

	bool AgvPeripheral::periphRecordPosWrite()
	{
		// TODO: shouldn't open/close every seconds.

		std::fstream fout(pos_record_file.c_str(), std::ios::out);
		if (fout.fail())
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "could not creat file %s.", pos_record_file.c_str());
			return false;
		}

		YAML::Node yamlConfig;
		yamlConfig["forklift_joint_pos"] = joint_pos;
		// RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "write jacking joint pos %.4f rotary joint pos %.4f", joint_pos[0], joint_pos[1]);
		fout << yamlConfig;
		fout.flush();
		fout.close();

		return true;
	}

	void AgvPeripheral::periphPosRecord()
	{
		if (IDLE_STATE == get_cur_state() || MANUAL_MODE_STATE == get_cur_state())
		{
			if (!periphRecordPosWrite())
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "write record pos fail");
			}
		}
	}

	void AgvPeripheral::thread_cycle()
	{
		periphPosRecord();
		check_jointState_out();
	}

	void AgvPeripheral::thread_fun(AgvPeripheral *pThis)
	{
		if (!pThis)
			return;

		while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(1000)))
		{
			pThis->thread_cycle();
		}
	}

}; // end namespace

/** @brief main function */
int main(int argc, char **argv)
{
	// Initialize the ROS node
	rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("agv_peripheral_controller");

	std::shared_ptr<agv_peripheral::AgvPeripheral> peripheral;
	std::shared_ptr<boost::thread> worker_thread;
	peripheral.reset(new agv_peripheral::AgvPeripheral("peripheral"));

	ROS_INFO_STREAM("peripheral fork wait 3s...");
	rclcpp::Duration(3).sleep();
	ROS_INFO_STREAM("peripheral fork wait 3s...ok");
	peripheral->init();

	worker_thread.reset(new boost::thread(&agv_peripheral::AgvPeripheral::spin, peripheral));

	rclcpp::Rate r(20);
	while (rclcpp::ok())
	{
		rclcpp::spin_some(node);
		r.sleep();
	}

	peripheral->shutdown();
	worker_thread->join();

	return 0;
}
